#include "system.h"

device_servo esc_left, esc_right, esc_back;
struct mpu6xxx_device *mpu6050=RT_NULL;
struct rt_device_pwm *pwm_stepper;

struct rt_mutex mutex_esc;

int restirct(int val, int ub, int lb){
    if (val > ub){
        return ub;
    }else if(val < lb){
        return lb;
    }else{
        return val;
    }
}

void esc_init(void* arg){
    /*双向电调通过上电时的信号确定双向/单向模式
    **首先将油门拉到最大，然后等待一段时间
    */
    rt_mutex_take(&mutex_esc, 100);

    servo_init(&esc_left, ESC_PWM_DEV, ESC_LEFT_PWM_CH, SERVO_MID, 0);
    servo_init(&esc_back, ESC_PWM_DEV, ESC_BACK_PWM_CH, SERVO_MID, 0);
    servo_init(&esc_right, ESC_PWM_DEV, ESC_RIGHT_PWM_CH, SERVO_MID, 0);

    rt_thread_mdelay(10000);

    rt_mutex_release(&mutex_esc);
}

void system_startup(void *arg){
    rt_mutex_init(&mutex_esc ,"mesc", RT_IPC_FLAG_PRIO);
}

void sensors_init(void *arg){
    /*初始化mpu6050*/
    mpu6050 = mpu6xxx_init(MPU6XXX_I2C_DEV, RT_NULL);
    mpu6xxx_set_param(mpu6050, MPU6XXX_ACCEL_RANGE, MPU6XXX_ACCEL_RANGE_4G);
    mpu6xxx_set_param(mpu6050, MPU6XXX_GYRO_RANGE, MPU6XXX_GYRO_RANGE_500DPS);
    struct mpu6xxx_3axes gyro_offset;
    gyro_offset.x = 86;
    gyro_offset.y = -29;
    gyro_offset.z = -51;
    mpu6xxx_set_gyro_offset(mpu6050, &gyro_offset);

    /*初始化sbus*/
    sbus_init(SBUS_UART_DEV);

    /*初始化UART端口*/
    ext_uart_init();
}

void stepper_init(void *arg){
    rt_pin_mode(STEPPER_DIR_PIN, PIN_MODE_OUTPUT);
    pwm_stepper = (struct rt_device_pwm*)rt_device_find(STEPPER_PWM_DEV);
    rt_pwm_set(pwm_stepper, STEPPER_PWM_CH, 1000000, 200000);
    rt_pwm_disable(pwm_stepper, STEPPER_PWM_CH);
}

void float_up(){
    rt_pin_write(STEPPER_DIR_PIN, PIN_HIGH);
    rt_pwm_enable(pwm_stepper, STEPPER_PWM_CH);
}

void dive_down(){
    rt_pin_write(STEPPER_DIR_PIN, PIN_LOW);
    rt_pwm_enable(pwm_stepper, STEPPER_PWM_CH);
}

void hang(){
    rt_pwm_disable(pwm_stepper, STEPPER_PWM_CH);
}

void sbusd(void *arg){
    int ch1, ch2, ch3, ch4;

    while (1){
        ch1 = sbus_read_channel(1);
        ch2 = sbus_read_channel(2);
        ch3 = sbus_read_channel(3);
        ch4 = sbus_read_channel(4);
        /*一副翼 二升降 三油门 四方向*/
        servo_write(&esc_left, sbus2servo(restirct(ch3+ch4-1000, 1800, 200)));
        servo_write(&esc_right, sbus2servo(restirct(ch3-ch4+1000, 1800, 200)));
        servo_write(&esc_back, sbus2servo(ch1));
        if (ch2 > 1200){
            float_up();
        }else if (ch2 < 800){
            dive_down();
        }else{
            hang();
        }
        rt_thread_mdelay(100);
    }
}
